ADiA24|6  M  ■  ^  , 

NAVAL  POSTGRADUATE  SCHOOL 
Monterey,  California 


THESIS 


ORIENTATION  GUIDANCE  AND  CONTROL  FOR 
MARINE  VEHICLES  IN  THE  HORIZONTAL  PLANE 

by 

Prouttichai  Suwandee 
June,  1991 

Thesis  Advisor:  Fotis  A.  Papoulias 

Approved  for  public  release;  distribution  is  unlimited. 


92-03487 


ECURIty  CLASSIfiCAIION  Of  THIS  PAGE 


REPORT  DOCUMENTATION  PAGE 


form  Approved 
OMBNo  0704  0188 


la  REPORT  SECURliy  ClASSIFICATION 

Unclassified 


2a  SECURITY  CLASSIFICATION  AUTHORITY 


2b  DECLASSIFICATION /DOWNGRADING  SCHEDULE 


4  PERFORMING  ORGANIZATION  REPORT  NUMBER(S) 


lb  RESTRICTIVE  MARKINGS 


3  DISTRIBUTION/AVAILABILITY  OF  REPORT 

Approved  for  public  release 
distribution  is  unlimited 


5  MONITORING  ORGANIZATION  REPORT  NUMBER(S) 


Sa  NAME  OF  PERFORMING  ORGANIZATION  I  6b  OFFICE  SYMBOL 

Naval  Postgraduate  Scholl 


6c  ADDRESS  (C/ty,  Stale,  and  ZIPCode) 


7a  NAME  OF  MONITORING  ORGANIZATION 

Naval  Postgraduate  School 


7b  ADDRESS  (City,  Slate,  and  ZIPCode) 


8a  NAME  Of  FUNDING /SPONSORING 
ORGANIZATION 


8b  OFFICE  SYMBOL  I  9  PROCUREMENT  INSTRUMENT  IDENTIFICATION  NUMBER 
(It  applicable)  | 


8c.  ADDRESS  (City,  Slate,  and  ZIP  Code) 


10  SOURCE  OF  FUNDING  NUMBERS 


PROGRAM  I  PROJECT  I  TASK 

ELEMENT  NO  I  NO  I  NO 


WORK  UNIT 
ACCESSION  NO 


1 1  TITLE  (Include  Security  Claisificalion) 

ORIENTATION  GUIDANCE  AND  CONTROL  FOR  MARINE  VEHICLES  IN  THE  HORIZONTAL  PLANE 


12  PERSONAL  AUTHOR(S) 

Prouttichai  Suwandee 


17  COSATI  CODES  I  18  SUBJECT  TERMS  (Continue  on  reveise  il  necessary  and  identify  by  block  number) 

I  group  I  SUB  GROUP  |  Autonomous  vehicles.  Guidance  and  control. 

Stability,  Path  keeping 


19  ABSTRACT  (Confini/e  on  reverse  if  necessary  and  identify  by  bfork  number) 

A  pure  pursuit  guidance  law  and  a  heading  autopilot  ue  coupled  in  order  to  provide 
path  control  of  submersibles  or  surface  ships  in  the  horizontal  plane.  Proper  design  of  the 
combined  scheme  elUows  for  accLuate  path  keeping  duruig  straight  line  motion.  The 
simulation  results  are  extended  to  cover  cases  of  step  changes  In  the  desired  path.  The 
scheme  provides  a  viable  alternative  to  cross  track  error  autopilots. 


20  DISTRIBUIION/AVAILABILITY  OF  ABSTRACT  21  ABSTRACT  SECURITY  ClASSIFICATION 

PI  UNCLASSIFIED'UNLIMITED  □  SAME  AS  RPT  □dTICUSERsI  I 


22«  NAME  Of  RESPONSIBIE  INDIVIDUAL  22b  TELEPIIONl  (liic/uc/f  Area  Code)  22c  OTfICf  SrMIIOL 

_ Fotis  A.  Papoulias  (408)  6463381  ME/PA 


DDForm  1473,  iUN  86  Pfev/ooJ  edff/ons  arp  o6so/ef?  _ SECURITY  CLASSif  iCAliprj  OF  7HiS  pA(»E_ 

S/N  0 1  ()2-LF-0  1  A-6603  Dnrl  rrc  i  f  i pH 


Unclassified 


i 


Approved  for  public  release;  distribution  is  unlimited. 


Author: 


Approved  by: 


Orientation  Guidance  and  Control  for  Marine 
Vehicles  in  the  Horizontal  Plane 


by 

Prouttichal  Suwandee 
Lieutenant  Conunander,  Royal  Thai  Navy 
B.S.,  Royal  Thai  Navy  Academy,  Thailand 

Submitted  in  partial  fulfillment 
of  the  requirements  for  the  degree  of 

MASTER  OF  SCIENCE  IN  MECHANICAL  ENGINEERING 

from  the 

NAVAL  POSTGRADUATE  SCHOOL 
June  1991 

'O  \ ,  5  J 


ProutHchai  Suwandee 


ABSTRACT 


A  pure  pursuit  guidance  law  and  a  heading  autopilot  are  coupled  in  order  to  provide 
path  control  of  submersibles  or  surface  ships  in  the  horizontal  plane.  Proper  design  of  the 
combined  scheme  allows  for  accurate  path  keeping  during  straight  line  motion.  The 
simulation  results  are  extended  to  cover  cases  of  step  changes  in  the  desired  path.  The 
scheme  provides  a  viable  alternative  to  cross  track  error  autopilots. 
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I.  INTRODUCTION 


In  any  operational  scenario  of  an  underwater  vehicle  there  exits  a  triple-nested 
sequence  of  mission  accomplishment  operations:  Path  planning,  navigation,  guidance,  and 
autopilot  design.  The  path  planner  takes  information  from  charted  obstacles  and  friendly 
or  hostile  environments  and  generates  a  smooth  plan  for  the  vehicle  to  follow.  A  certain 
level  of  feedback  exists  in  this  operation  through  the  use  of  sonar  beams  in  order  to 
replan  a  path  when  uncharted  objects  are  encountered  or  when  the  mission  requirements 
have  changed.  Based  on  the  desired  vehicle  positions  and  orientations  at  certain  points, 
several  classes  of  smooth  paths  containing  sets  of  straight  line  segments,  and  circular  arcs 
or  cubic  splines  can  be  obtained  [1].  Once  a  smooth  path  is  generated,  the  navigator 
provides  through  a  selected  guidance  law  the  appropriate  vehicle  heading  commands 
which  are  in  turn  delivered  by  the  autopilot.  Line  of  sight  guidance  using  a  discrete  series 
of  way  points  was  studied  by  Lienard  in  [2]  using  sliding  mode  heaving,  propulsion,  and 
depth  keeping  autopilots.  The  scheme  demonstrated  excellent  stability  and  robusmess 
characteristics,  although  the  actual  vehicle  response  was  found  to  lag  significantly  the 
commanded  straight  line  paths.  The  guidance  and  autopilot  functions  can  be  combined 
when  the  lateral  deviation  off  the  desired  path  is  directly  incoiporated  into  the  control  law 
design.  This  leads  to  the  development  of  a  cross  track  error  autopilot.  Such  schemes  have 
been  studied  by  Chism  [?]  and  Hawkinson  [4]  for  the  single  input  and  multiple  input 
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case,  respectively.  Cross  track  error  autopilots  provide  more  accurate  path  keeping 
response  but  they  must  be  designed  more  carefully  since  they  tend  to  be  more  dependent 
than  heading  controllers  on  an  accurate  description  of  the  vehicle  hydrodynamic 
characteristics.  The  main  reason  for  this  is  the  increase  in  the  system  dimensionality  by 
one.  Underwater  vehicles  operate  in  changing  environments  over  a  wide  range  of 
operating  speeds  and,  therefore,  a  certain  degree  of  uncertainty  exists  in  the  vehicle 
dynamic  modeling.  Cross  track  error  autopilots  also  require  accurate  positional 
information  updates  at  the  same  rate  as  heading  and  heading  rate. 

For  these  reasons,  in  this  work  we  go  back  to  the  case  of  a  heading  autopilot 
coupled  with  an  orientation  guidance  law.  The  two  main  tasks  on  which  we  will 
concentrate  are  as  follows:  First,  we  must  develop  a  way  of  establishing  stability  of  the 
combined  autopilot/guidance  scheme  for  straight  line  commanded  motions.  Second,  the 
actual  vehicle  response  characteristics  must  be  made  to  resemble  the  desired  cross  track 
error  response  with  smooth  transitions  between  consecutive  straight  line  segment  and  with 
minimal  path  oveishoot.  A  linear  full  state  feedback  control  law  is  used  to  adjust  the 
heading  of  the  vehicle  to  any  desired  value,  and  a  pure  pursuit  guidance  law  is  used  to 
provide  the  commanded  heading  for  straight  line  motion.  In  this  scheme  the  vehicle 
commanded  heading  equals  the  line  of  sight  angle  between  the  vehicle  center  and  a  target 
point  moving  on  the  desired  path  at  a  constant  lookahead  distance  from  the  vehicle.  This 
parallels  the  case  studied  in  [5]  except  that  in  our  case  the  existence  of  lateral  and 
rotational  dynamics  add  more  complications  to  the  problem.  All  computations  aie 
performed  for  the  Swimmer  Delivery  Vehicle  [6]  for  which  a  set  of  hydrodynamic 
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characteristics  and  geometric  properties  is  available.  Problem  formulation  and  equations 
of  motion  are  presented  in  Section  n.  The  analysis  procedure  is  outlined  in  Section  ID, 
and  simulation  results  are  presented  in  the  Section  IV.  Finally,  conclusions  and 
recommendations  for  further  research  are  given  in  Section  V. 


3 


n.  DEVELOPMENT  OF  THE  MATHEMATICAL  MODEL 


A.  EQUATIONS  OF  MOTION 

In  a  moving  coordinate  frame  fixed  at  the  vehicle  center,  Newton’s  equations  of 
motion  for  a  rigid  body  in  the  horizontal  plane  are 

m{v  +ur  -y^r^)  =  Y  ,  (2.1) 

*ur) -my^vr  =  N  ,  (2.2) 


where 

V  =  sway(lateral)  velocity, 
r  =  yaw(angular)  velocity, 
u  =  forward(surge)  speed, 

Y  =  sway  force. 

N  =  yaw  moment, 
m  =  vehicle  mass, 

Iz  =  vehicle  mass  moment  of  inertia, 

(^>yo)  =  coordinates  of  center  of  gravity. 

Expanding  the  force  Y  and  moment  N  in  added  mass,  damping,  and  drag  tenns. 
equations  (2.1)  and  (2.2)  are  written  as 
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m(v  +  ur  +xj  -yr  =  £./  *Yr  +  —/ \Yy  +  Y^ur) 


l/^Yuv-l  fc,  .  £/ V  ^5, 

2  '  2J  I  I  2 


(2.3) 


V  +xr 


+mjCg(v +ur) -mjgVr  =  ^l^Nr +  £.l\N\’ +Nur) 


uv  -R.  fC™  hjx)  xdx  +2.1  ^yM ^5, 

2  ■'  iJ  I  I  2 


(2.4) 


V  +jcr 


where 

p  =  water  density, 

1  =  vehicle  length, 

5  =  rudder  angle, 

h(x)  =  vehicle  height  distribution, 

Cpy  =  drag  coefficient. 

The  inertial  position  of  the  vehicle  (x,y)  and  its  heading  angle  y  (  see  Figure  1  ) 
are  given  by 

y  =  r  ,  (2-5) 

X  -  ucosy-vsiny  ,  (2.6) 

y  =  Msiny +vcosy  . 
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Figure  1  Top  view  of  the  vehicle 


B.  STATE  SPACE  EQUATIONS 


Choosing  (  y,  v,  r  )  as  the  state  vector,  the  linearized  state  space  equations  (2.3), 
(2.4),  and  (2.5)  are  written  as 


y  =  r  , 


(2.8) 


V  =  a,,uv+a,2ur+&,u^8  , 


(2.9) 


r  =  a^^uv+a^ur+b^u^b  . 


(2.10) 


The  coefficients  in  equations  (8),  (9),  and  (10)  are  given  by 


(/ -0.5p/  W.)(0.5p/ \v..)-(mX^-0.5p/  ‘>v)(0.5p/  W ) 
{I^-0.5pl%)im-0.5pl\,)-{mX^-<).5pl^y,)(mX^-0.5pl*N,) 

(/.-0.5p/*/V,)(-m+0.5p/V-("*XG-0-5p/W-mX^+0.5p/X) 

iI^-0.5pl%){m-<).5pl%)-{na^-0.5pl\XmX^-0.5pl*N,j 

(mX^-0.5p/‘y,)(0.5p/W,)-(mXc-0.5p/^/V.)(-m+0.5p/^y„) 

(/,  -0.5p/  ’/V.)(m  -0.5p/  ^y.)  -{mX^-0.5pl  *y-XmX^  -0.5pl  *N.) 

(mXg  -0.5p/  *y.)i  -mX^  +0.5p/  X)  +0.5p/  \v^) 

“l/-0.5p/’N,)(m-0.5p/^y,)-(mX^-0.5pry.)(mX^-0.5p/^yV,) 

(/.-0.5p/  ’Ar.)(0.5p/  ^y8)-(mX^-0.5p/  *y,)(0.5p/  W*) 

(/.-0.5p/ ’yv.)(m -0.5p/ \.)-{mX^-0.5pl *y.){niX^-0.5pl  ) 
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imX^-0.5pl  *y,)i0.5pl  ^N,)-imX^-0.5pl  %)(.0.5pl  \) 
{I^-i).5pnN,Km-0.5pl%)-iniX^-0.5pl\){rnX^-0.5pl%) 


K  = 

where 

Vr’  Vv'  yr’  Vi  ~  hydrodynamic  coefficients 

Nf^,  N^,  N.,  N.  =  yaw  hydrodynamic  coefficients 

Equations  (2.8),  (2.9),  and  (2.10)  describe  the  dynamics  of  the  system  with  respect 
to  small  deviations  around  a  nominal  direction  y  =  0  . 

C.  PATH  KEEPING  DEVELOPMENT 
1.  Heading  control 

A  linear  full  state  feedback  control  law  is  of  the  form 

6  =  k^^\^+k^v+k/  (2.11) 

where  k),  kj  and  kj  are  the  three  gains. 

From  equation  (2.8),(2.9),(2.10)  and  (2.11),  the  closed  loop  system  is 

r  (2.12) 

6,u  ^k^^^+{a^^u+b^u  ^kj)v+{a^^u*b^u^k^r  (2.1 3) 
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r  =  b^u\y^^{a^^u+b^u\)v+{a^u-^b^u\)r 


(2.14) 


The  characteristic  equation  of  (2.12),  (2.13)  and  (2.14)  is 


0-X 

0 

1 

b,u\ 

a^^u-^b^u^kj^-k 

a^^u*b,u% 

b,u% 

a^^u+bju  \ 

a22U+bju\~k 

0 


X[{q^^u  +b^u  \  -X)ia^u  +b^u  \  -"kyia^^u^b^u  +b^u  \)] 

-b^u'^k^{a^^u^b^u‘^k^*bJu'^k^ia^^u+b^u'^kJ^-k)  =  0 

'k[a^^a^u^■*■a^fi^u  ^k^-ka^^u+b^a^u  ^k^+bfi^u  *k^k^-kb^u  ^k^-ka^^u 
-XfcjM  %+k^-a^202tU^-^i2^2^  ’^2  ■^21^1“ 

-b^a^^u\-b^bJn\k^*a^^bJii^k^*b^b^u*k^k^-}J>^u‘^k^  -  0 

V  -ia^^u  +byU  \  +a^u  +bjU  ^kj)k^  +(a,  jajjM  ^  +^,,^>2“  ’^3  ^^2 

-a^^a^^u^-a^^bJit^k^-a^^b^u^k^-b^u^k^k*b^a^^u^k^-b^a^^u^k^  =  0  (2.15) 


2.  Desired  characteristic  equation 

The  3rd  order  ITAE  polynomial  is  defined  by 


X’+1.75(D„A.^+2.15o^X+(o’  =  0 


(2.16) 
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where 


=  settling  time  (dimensionless) 
From  equations  (2.15)  and  (12.6)  we  get 


-a^^u-b^u^k^-a^u-bji^k^  =  1.75<Do 


(2.17) 


a,  ,^22“  '  '*3  ^^.«22“  \  -^.2^2, “  ' 

-a^^b^u^k^-a^fi^u^k^-bJU^k^  =  2.15(1^  (2.18) 


Vi.“'*.“V2i“'^i  =  «b 


(2.19) 


The  system  of  equations  (2.17),  (2.18)  and  (2.19)  can  be  solved  for  the  three  gains 

.  <0^ 

Ar,  =  - 

Vn«'-V2i«' 

^  _  u  ‘‘(-1.75®„-a„  -aj2){a^^b^-a^p^)-b^u  2(2.15(0b-a-l  la^^u^-^a^^a^y+b^u  %) 

b,u  \a^p^-a^p;)-b^u  \a^b, 

^  _  bjU  ^[2.  15(Oo-m  ^(flt,  -a^^a^^)+b^u  -u  \  -1  J5G:^-a^^u-a^uy{a2JJ^  -0,2^2^ 

b,«  ’(a„fr2-«21^l)"^“  ’(«22^I  -«12^2> 
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3.  Pure  pursuit  navigation 
For  a  pursuit  navigation 


=  a 


(2.20> 


where 


=  commanded  heading 

Referring  to  Figure  1 ,  the  line  of  sight  angle  a  is  denned  by 

C  =  -tan"*— 

where  is  the  vehicle  lookahead  distance,  and  the  control  lav/  (2.11)  becomes 


(2.21) 


8  =  (2.22) 

Using  equations  (2.20),  (2.21)  and  (2.22)  we  get 
8  =  k,{\u+tan''—)->-k.v+kj- 

‘  ^  x/  ^  '  (2.23) 

The  linearized  equation  (2.23)  becomes 

8  =  *,(y+jL)+it,v+lr,r 

'  ^  x/  '  '  (2.24) 

The  linearized  equation  for  the  lateral  deviation  y  is  obtained  from  (2.7)  as 

y  =  My+v  (2.23) 
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Now  the  complete  state  vector  is  y,  v,  r  and  y  ,  and  the  state  space  equations  are  written 


as 


y  =  r 

r  =  bju^k^\f+{a^^u^b^u'^k^)v-^ajju+b^u'^k^)r+bju^k^—y 

^d 


(2.26) 


(2.27) 


(2.28) 


y  =  uy+v 


and  the  characteristic  equation  is 


(2.29) 


0-X 

0 

1 

0 

a^^u+b^u^k^-\ 

a^2U+b^u^k, 

b,u%l 

*d 

bju  ^kj 

aj^^u+bju^k^ 

a^u+b^u^kj-X 

b^u%L 

^d 

u 

1 

0 

o-x 

(2.30) 
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in.  COMPUTER  SIMULATION 


A.  PROGRAMMING 

1.  Program  in  MATRIX.X 

The  MATRIX.X  software  is  in  VAX/VMS  in  the  Mechanical  Engineering 
I>epartment.  The  program  in  MATRIX.X  is  written  to  find  gains  and  poles  of  the  system 
from  the  given  inputs,  forward  speed  (u),  settling  time  (t,.)  and  vehicle  lookahead  distance 
(Xj).  This  is  used  to  compute  the  eigenvalues  of  the  complete  system  (  Equation  (2.30), 
The  four  eigenvalues  of  the  system  are  computed  for  the  given  values  of  u,  t,.  and  Xj. 
For  stable  vehicle  response,  all  four  must  be  negative  (or  have  negative  real  parts).  If  at 
least  one  is  positive,  the  vehicle  response  will  be  unstable  and  convergence  to  the  straight 
line  path  is  not  to  be  expected.  In  such  a  case,  the  parameters  (in  particular  the  lookahead 
distance  must  be  changed)  so  that  the  vehicle  is  stable.  A  listing  of  this  program  is 
presented  in  Appendix  A. 

2.  Program  in  FORTRAN 

The  first  program  in  FORTRAN,  is  written  to  find  distance  along  body  fixed 
axis  (  x-y  )  from  inputs,  heading  angle  (y),  perpendicular  distance  from  vehicle  to  route 
(y),  yaw  rate  (r),  forward  speed  (u),  settling  time  (t^)  and  vehicle  lookahead  distance  (x^). 
A  modified  version  of  this  simulation  program  is  presented  in  Appendix  B.  The  modified 
version  is  used  to  control  the  vehicle  position  in  a  general  (  X-Y  )  inertial  system.  The 
desired  vehicle  path  is  discretized  into  a  series  of  straight  line  segments  and  the  same 
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lookahead  distance  Xj  is  used  to  regulate  the  vehicle  deviation  of  each  segment.  Details 
of  this  modification  are  presented  in  the  next  paragrtq^h. 

3.  Graphics 

The  GRAFSTAT  graphic  package  is  used  to  produce  2  dimensional  graphs 
by  using  data  from  the  simulation  programs. 


B.  DETAILS 

1.  Poles  and  gains 

Calculate  the  system  poles  and  gains  for  a  given  forward  speed  u  and  various 
combinations  of  t^  and  Xj.  Select  t^  and  Xj  such  that  appropriate  (sufficiently  negative) 
poles  and  gains  for  the  system  are  produced.  This  is  verified  by  repeated  simulations  from 
step  2  that  follows 

2.  Distance  in  x<y  axis 

Using  the  values  of  t^  and  Xj  from  the  previous  step,  the  system  response  can 
be  simulated.  Unlike  the  control  law  design,  the  simulation  is  based  on  the  full  nonlinear 
equations  of  motion  for  the  vehicle,  (3),  (4),  (5)  and  (7).  Typically,  the  initial  conditions 
consist  of  nonzero  values  of  the  lateral  deviation  y  and  heading  Y- 

3.  Distance  in  X>Y  axis 

A  similar  procedure  is  used  to  simulate  the  vehicle  response  in  a  general  path 
in  the  X-Y  plane,  as  shown  in  Figure  2.  The  perpendicular  distance  y  from  the  vehicle 
to  the  desired  route  in  the  X-Y  plane  is  used  to  compute  the  commanded  heading  angle 
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The  difference  where  a  is  the  angle  of  the  route  with  respect  to  the  X-axis,  is 
used  instead  of  the  heading  Y  in  the  control  law.  The  above  computations  are  performed 
by  appropriate  coordinate  rotations  between  the  two  coordinate  systems. 

When  transiting  from  one  straight  line  path  to  the  next,  the  same  lookahead  distance 
Xj  is  used  for  both  segments.  The  vehicle  switches  to  the  next  segment  when  it  gets 
within  a  specified  distance  from  the  terminal  way  point.  This  distance  is  measured  along 
the  x-axis  and  for  given  t^  and  x^,  and  it  should  increase  as  the  angle  a  for  the  next 
segment  increases.  Too  high  or  too  low  values  of  this  turning  distance  result  in  path 
overshoot  and  undesirable  oscillatory  response.  The  optimum  turning  distance  that  allows 
for  the  smooth  transition  between  consecutive  straight  line  paths  is  established  with  the 
aid  of  the  simulation  program  from  step  2  as  follows. 

For  a  fixed  initial  heading  y,  the  initial  deviation  y  is  varied  until  the  vehicle 
response  is  smooth  and  sufficiently  fast  with  no  path  overshoot.  The  process  is  repeated 
for  different  initial  conditions  in  Y  2nd  a  curve  in  y  versus  Y  is  constracted.  This  is 
shown  in  Figure  3  and  is  the  desirable  turning  distance  versus  turning  angle  curve.  The 
actual  curve  is  approximated  by  two  straight  lines  which  are  used  to  initiate  the  turn  in 
the  simulation  program.  A  copy  of  this  simulation  program  is  included  in  Appendix  B. 
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TURNING  DISTANCE  VERSUS  TURNING  ANGLE 


Figure  3  Turning  distance  and  turning  angle 
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IV.  SIMULATION  RESULTS 


The  vehicle  parameters  used  in  the  simulation  are: 


X 

m 

= 

-0.0076 

= 

0.0012 

X 

rr 

= 

0.0040 

y-v 

= 

-0.0550 

= 

0.0200 

= 

0.0300 

= 

-0.0010 

V 

^  V 

= 

-0.1000 

= 

0.0530 

= 

0.0270 

= 

0.00173 

^Dy 

= 

0.3500 

= 

-0.1000 

W 

= 

12000 

lb. 

n 

r 

-0.0034 

1 

s 

17.4 

ft- 

n 

V 

= 

0.0012 

P 

s 

1.94 

slugtft^. 

n 

r 

= 

-0.0160 

8 

= 

32.2 

ft./stc. 

n 

V 

s 

-0.0074 

I 

t 

10000 

ft*- 

= 

-0.0130 

V 

= 

0.000847 

ft^./stc 

The  simulation  begins  by  setting  y  =  5  degrees,  \,  =  2  vehicle  lengths,  t^  =  5,  r 
=  0,  V  =  0  ft./sec,  u  =  5  ft./sec.  and  y  =  1 .  When  the  vehicle  moves  to  a  distance  x  =  20 
then  the  simulation  stops.  The  route  of  vehicle  and  the  rudder  angle  (6)  that  vehicle  used 
during  simulation  are  shown  in  Figure  4.  The  heading  angle  (y)  and  commanded  heading 
(y^)  are  shown  in  Figure  5.  Yaw  velocity  and  sway  velocity  are  shown  in  Figure  6. 
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The  values  for  and  Xj  were  selected  based  on  the  results  of  the  previous  chapter. 
From  the  figures  it  can  be  seen  that  the  vehicle  response  is  very  fast  with  limited 
overshoot.  This,  of  course,  depends  heavily  on  the  initial  conditions  of  the  simulation.  The 
actual  heading  angle  converges  rather  rapidly,  after  the  initial  transients  have  died  out,  to 
the  commanded  heading  angle. 

The  second  scries  of  simulations  was  performed  in  order  to  assess  the  capability 
of  the  control  and  guidance  law  to  change  course  and  keep  the  new  path.  Simulation 
parameters  were  x^  =  2,  t^  =  5  and  u  =  5  as  before.  Initial  conditions  for  the  simulations 
were  y  =  0,  r  =  0  and  v  =  0  with  the  initial  vehicle  position  at  (X^.Yo)  =  (5,0)  in  the 
global  reference  frame.  The  first  straight  line  segment  is  determined  by  the  way  points 
(5,0)  and  (25,0)  and  the  second  by  (25,0)  and  (67.89,20).  For  this  route  the  corresponding 
course  change  is  25  degrees.  The  results  of  this  simulation  are  presented  in  Figure  7 
where  along  with  the  actual  vehicle  path,  a  side  path  at  distance  of  1  vehicle  length  off 
the  desired  path  is  shown.  This  corresponds  to  an  arbitrary  "  safety  path  band  "  for  the 
vehicle.  The  turning  distance  was  fixed  at  2  vehicle  lengths  throughout  the  simulations. 
From  Figure  7  it  can  be  seen  that  the  vehicle  turns  to  the  new  course  smoothly  with  no 
path  overshoot.  When  the  second  route  changes  to  (25,0)  and  (41.78,20)  which 
corresponds  to  50  degrees  course  change,  the  results  of  Figure  8  show  that  a  path 
overshoot  occurs  although  it  is  yet  not  serious  enough  according  to  the  artificial  safety 
criterion  described  above.  However,  when  the  second  route  changes  to  (25,0)  and 
(30.36,20)  which  corresponds  to  75  degrees  course  change,  significant  vehicle  oscillatory 
response  and  side  path  overshoot  occurs,  as  demonstrated  in  Figure  9. 
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The  above  simulations  demonstrate  the  need  for  adjustable  turning  distance; 
although  path  accuracy  is  obtained  regardless  of  the  value  of  the  turning  distance,  the 
transient  response  during  course  change  is  not  always  within  some  predetermined  safety 
bounds.  For  this  reason  we  employ  the  built-in  turning  distance  versus  turning  angle 
relationship  shown  in  Figure  3  and  repeat  the  simulations  for  the  aforementioned  three 
course  changes.  The  results  are  shown  in  Figures  10, 1 1  and  12,  where  it  can  be  seen  that 
the  vehicle  response  is  now  satisfactory  for  both  course  keeping  and  course  changing. 

Finally,  the  last  simulation  test  was  performed  in  order  to  establish  the  capabilities 
of  the  scheme  to  follow  a  general  path  in  the  horizontal  plane.  For  demonstration 
purposes  the  path  was  assumed  to  consist  of  the  way  points  (0,10),  (5,10),  (25,0),  (35,20), 
(50,20),  (70,10),  (50,-10)  and  (30,-10).  Straight  line  segments  were  assumed  as  the  desired 
paths  between  consecutive  way  points.  The  same  simulation  parameters  were  used  as  in 
the  previous  runs.  The  results  of  this  simulation  are  presented  in  Figures  13  for  adjustable 
turning  distance,  and  14  for  the  fixed  turning  distance  case.  It  can  be  clearly  seen  that 
when  the  turning  distance  is  function  of  turning  angle,  the  scheme  achieves  excellent  path 
keeping  characteristics  with  smooth  course  changes  and  minimal  path  overshoot. 
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Figure  4  Pursuit  navigation 
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Figure  5  Pursuit  navigation 
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Figure  6  Pursuit  navigation 
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Y  POSITION 
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Figure  7  Path  control 
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Y  POSITION 


TURNING  ANGLE  50  DEGREES 


TURNING  DISTANCE  CONSTANT 


Figure  8  Path  control 
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TURNING  ANGLE  75  DEGREES 
TURNING  DISTANCE  CONSTANT 


Figure  9  Path  control 
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Y  POSITION 


TURNING  ANGLE  25  DEGREES 
TURNING  DISTANCE  FUNCTION  OF  TURNING  ANGLE 


Figure  10  Path  control 
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Y  POSITION 


TURNING  ANGLE  50  DEGREES 
TURNING  DISTANCE  FUNCTION  OF  TURNING  ANGLE 


Figure  1 1  Path  control 
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Y  POSITION 


TURNING  ANGLE  75  DEGREES 
TURNING  DISTANCE  FUNCTION  OF  TURNING  ANGLE 


Figure  12  Path  control 
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Y  POSITION 


TURNING  DISTANCE  FUNCTION  OF  TURNING  ANGLE 
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Figure  13  Pure  pursuit  navigation 
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Figure  14  Pure  pursuit  navigation 
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V.  SUMMARY  AND  CONCLUSIONS 


The  piiiicipal  conclusions  of  this  woik  can  be  summarized  as  follows: 

1 .  Orientation  control  law  can  be  used  in  order  to  provide  accurate  vehicle  path  keeping 
when  combined  with  an  appropriate  guidance  scheme. 

2.  Pure  pursuit  guidance  was  found  to  work  very  well  for  the  autonomous  underwater 
vehicle  case  and  its  simplicity  make  it  a  very  attractive  alternative  to  cross  track  error 
schemes. 

3.  A  built-in  turning  distance  versus  course  change  relationship  can  be  utilized  to  initiate 
the  turn  at  the  appropriate  time  in  order  to  avoid  path  overshoot  and  achieve  smooth  path 
transitions. 

4.  It  is  expected  that  the  added  robustness  that  heading  schemes  naturally  enjoy  will  aid 
in  maintaining  stability  in  cases  where  incomplete  and  inaccurate  vehicle  dynamic 
descriptions  are  available. 

Some  recommendations  for  further  research  are  as  follows: 

1 .  Comparative  studies  must  be  performed  with  other  orientation  guidance  schemes  such 
as  proportional  navigation  and  also  with  velocity  guidance  laws  in  order  to  ensure  that 
the  best  technique  is  ultimately  employed. 
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2.  Similar  studies  must  be  performed  in  the  vertical  plane.  Combined  with  the  horizontal 
plane  techniques  developed  in  this  work  and  with  propulsion  control  they  could  be 
utilized  to  provide  accurate  trajectory  following  in  3-D  space. 
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APPENDIX  A. 


1-17.425; 
inquire  xd_l 
inquire  tc 
inquire  u 
xd=xd_l *1 ; 
aall  — 0.045380; 
aal2— 0.351190; 
aa21— 0.002795; 
aa22— 0.095680; 
bbl-  0.011432; 
bb2  —  0.004273; 

OMEGA- ( 10 .0*U)/(TC*L) ; 

ADl-1 .75*OMEGA; 

AD2-2 . 15*OMEGA**2 ; 

AD3-OMEGA**3; 

Al-BB1*U*U; 

Bl=BB2*U*U; 

C1—AD1-(AA11+AA22)*U; 

A2-{BB1*AA22-BB2*AA12)*U**3; 

B2-(BB2*AA11-BB1*AA21)*U**3; 

Kl-AD3/( (BB2*AA11-BB1*AA21 )*U**3) ; 

C2-AD2-(AA11*AA22-AA12*AA21)*U**2+BB2*U*U*K1 ; 

K2-(C1*B2-C2*b1 )/(Al*B2-A2*Bl) ; 

K3-(C2*A1-C1*A2 )/( Al*B2-A2*Bl ) ; 

all-0; 

al2-0; 

al3-l; 

al4-0; 

a21-bbl*u*u*kl; 
a22=aall*u+bbl*u*u*k2; 
a23-aal2*u+bbl*u*u*k3 ; 
a24-bbl*u*u*kl/xd ; 
a31-bb2*u*u*kl ; 
a32-aa21*u+bb2*u*u*k2; 
a33-aa22*u+bb2*u*u*k3 ; 
a34-bb2*u*u*kl/xd ; 
a41-u ; 
a42-l ; 
a43-0; 
a44-0; 

a-lali,al2,al3,al4;a21,a22,a23,a24; 

a31,a32,a33,a34;a41,a42,a43,a44]; 

eig(a) 
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APPENDIX  B. 


PROGRAM  SUB. FOR 

PROUTTICHAI  SUWANDEE 
NAVAL  POSTGRADUATE  SCHOOL 
MARCH  1991 

AUV  LINE-OF-SIGHT  NAVIGATION  AND  CONTROL 
VARIABLE  GAINS  INTERNALLY  COMPUTED 

REAL  L,MASS,NRDOT,NVDOT,NR,NV,NDR 
REAL  IZ,NU,LLL,NSL,K1,K2,K3 

DIMENSION  X(9) ,HH(9) , VECl { 9 ) , VEC2 ( 9 ) , TT( 1000 ) , YY( 6 , 1000 ) , 
*ALPHA(10) ,XZ(10) ,Y2(10) 

LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER  (  XRR» 4  .  E-  3  ,  XUDOT— 7 . 6E-  3  ,  XVR-2  .  E-2  ,  XRDR—  1 .  E-  3  , 

1  XVV-5 . 3E-2 , XVDR-1 . 7E-3 , XDRDR— 1 . E-2 ) 


LATERAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER ( YRDOT-1 . 2E-3 , YVDOT*-5 . 5E-2 , YR-3 . E-2 , YV=-1 . E-1 , 

1  YDR=2.7E-2,CDY-3.5E-1) 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER! NRDOT-- 3 . 4E-3 ,NVDOT»l . 2E-3 ,NR— 1 . 6E-2 ,NV— 7 . 4E-3 , 
1  NDR— 1.3E-2) 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 

PARAMETER! WEIGHT=12000 . ,XG  *  0 . , IZ-10000 . , L-17 . 4 , RHO=l . 94 , 

1  G-32.2,NU>-8.47E-4) 

OPEN  !10,FILE='SUB.IN' , STATUS- ' OLD ' ) 

OPEN  ! 1 1 , FILE- ' SUB . OUT ' , STATUS- ' OLD ' ) 

READ  !10,*)  TARGET 

READ  !10,*)  TSIM, DELTA,  IPRH'T' 

READ  !10,*)  PSI,R 
READ  !10,*)  U 
READ  !10,*)  TC,VC 

NUMBER  OF  POSITION  OF  ROUTE  AND  POSITION  OF  VEHICLE 


READ  (10,’")  N,XO,YO 
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POSITION  OF  ROUTE  IN  X-Y  AXIS 
DO  30  I»1,N 

READ  (10,*)  XZ(I),yZ(I) 

30  CONTINUE 

TARGET=TARGET*L 
TWOPI  -8.0*ATAN(1.0) 

PI  =0.5*TWOPI 

DO  999  M=1,N-1 

PSI1=0 

YTURN=0 

XYTURN-0 

PSI-PSI*PI/180 . 0 

MOVE  ORIGINE  OF  AXIS  TO  THE  FIRST  POINT  OF  THE  ROUTE 

XF=XZ(M+1 )-XZ(M) 

IF  (XF.EQ.O)  XF-0. 0000001 
YF-YZ(M+1 )-YZ(M) 

XO»XO-XZ(M) 

IF  (XO.EQ.O)  XO-0. 0000001 
YO=YO-YZ(M) 

ANGLE  OF  POSITION  OF  VEHICLE 
ALPHA0=ATAN ( ABS ( YO/XO ) ) 

IF  ( (YO.GT.O) .AND. (XO.LT.O) )  ALPHAO-PI-ALPHAO 
IF  ( ( YO.LE.O) .AND. (XO.GT.O) )  ALPHA0-2*PI-ALPHA0 
IF  ( (YO.LE.O) .AND. (XO.LT.O) )  ALPHAO-Pl+ALPHAO 

ANGLE  OF  ROUTE 

ALPHA1=ATAN(ABS( YF/XF) ) 

IF  ( (YF.GT.O) .AND. (XF.LT.O) )  ALPHAl»PI-ALPHAl 
IF  ( (YF.LE.O) .AND. (XF.GT.O) )  ALPHAl-2*PI-ALPHAl 
IF  ( (YF.LE.O) .AND. (XF.LT.O) )  ALPHAl-PI+ALPHAl 

ANGLE  BETWEEN  VEHICLE  AND  ROUTE 

BETA=(ALPHA0-ALPHA1 ) 

DISTANCE  FROM  ORIGINE  TO  VEHICLE 

RT=( XO**2+YO**2 ) **0 . 5 

PROJECTED  DISTANCE  FROM  ORIGINE  TO  VEHICLE  ON  THE  ROUTE 


XS=RT*COS( BETA) 

XC=( XS ) *COS( ALPHAl ) 
YC=( XS ) *SIN( ALPHAl ) 
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PERPENDICULAR  DISTANCE  OF  VEHICLE  TO  X-AXIS 

YPOS=RT*SIN( BETA) 

TOTAL  DISTANCE  ON  THE  ROUTE 

TXD-(XF**2+YF**2)**0.5 
XD=ABS(TXD-XS) 

HEADING  ANGLE  • 

IF(M.EQ.l)  PSI-PSI-ALPHAl 

IF  (PSI.GT.PI)  PSI-PSI-2*PI  ’ 

IF  (PSI.LT.-PI)  PSI-PSI+2*PI 
IF  (M.EQ.N-1)  GO  TO  65 

NEXT  HEADING  ANGLE 

DUMY=YZ ( M+2 ) -YZ ( M+1 ) 

DUMX=XZ ( M+2 ) -XZ ( M+1 ) 

IF  (DUMX.EQ.O)  DUMX-0. 0000001 
ALP2=ATAN ( ABS ( DUMY/DUMX ) ) 

IF  ( (DUMY.GT.O) .AND. (DUMX.LT.O) )  ALP2-PI-ALP2 
IF  (  (DUMY.LE.O)  .AND.  (DUMX.GT.O)  )  ALP2-.'' *PI-ALP2 
IF  ( (DUMY.LE.O) .AND. (DUMX.LT.O) )  ALP2-ri+ALP2 
PSI1-ALPHA1-ALP2 
IF  (PSIl.GT.PI)  PSI1=PSI1-2*PI 
IF  (PSIl.LT.-Pl)  PSI1=PSI1+2*PI 
PSI1=PSI1*180/P1 

TURNING  DISTANCE 

IF(ABS(PSI1) .LE.45)  YTURN=ABS( PSIl/25 ) 

IF(ABS(PSI1),GT.45)  YTURN= (ABS(PSIl)-45)*0.3/5+1.8 
XYTURN=YTURN/ABS( SIN( PSI1*PI/180 ) )-0 . 2 
IF(PSIl.EQ.O)  XYTURN=0.0 

65  UC  -U 

OMEGA=( 10 . 0*U)/(TC*L) 

ADI  =1.75*OMEGA 
AD2  =2 . 15*OMEGA**2 

AD3  =OMEGA**3 
C 

PISIM  =TSIM/DELTA 
ISIM  =PISIM 

ECHO  = 1.0/DELTA  ^ 

lECHO  =IPRNT*20 

YAW  =0 . 0 

SWAY  =0.0 

V  =0.0 

DR  =0.^ 

R  =R*PI/180.0 
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n  n  n 


ISTART=1 
XPOS  -0.0 
YPOS  -YPOS*L 


DEFINE  THE 


LENGTH  X  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 


C 


C 

c 


c 


c 


X(l)  -  -105.9/12. 

X(2)  =  -99.3/12. 

X(3)  -  -87.3/12. 

X(4)  =  -66.3/12. 

X(5)  -  72.7/12. 

X(6)  -  83.2/12. 

X(7)  -  91.2/12. 

X(8)  -  99.2/12. 

X(9)  -  103.2/12. 

HH(1)  =  0.00/12. 

HH(2)  -  8.24/12. 

HH(3)  -  19.76/12. 

HH(4)  -  29.36/12. 

HH(5)  -  31.85/12. 

HH(6)  -  27.84/12. 

HH(7)  -  21.44/12. 

HH(8)  -  12.00/12. 

HH(9)  -  0.00/12. 

MASS  -  WEIGHT/G 

Pl-MASS-0. 5*RHO*L**3*XUDOT 
P3-MASS-0.5*RHO*L**3*YVDOT 
P4-MASS*XG-0.5*RHO*L**4*YRDOT 
P5-IZ-0.5*RHO*L**5*NRDOT 
P6-MASS*XG-0 . 5*RHO*L**4*NVDOT 
D  -P5*P3-P4*P6 

AAll-( P5*0 . 5*RHO*L*L*YV-P4*0 . 5*RHO*L**3*NV)/D 
AA12-( P5* ( -MASS+0 . 5*RHO*L**3*YR)-P4* ( -MASS*XG 
1  +0 . 5*RHO*L**4*NR) )/D 

AA21-( P3*0. 5*RHO*L**3*NV-P6*0.5*RHO*L*L*YV)/D 
AA22={ P3* ( -MASS*XG+0 . 5 *RHO*L* * 4*NR ) -P6 * ( -MASS 
1  +0 . 5*RHO*L**3*YR) )/D 

BB1=( P5*0 . 5*RHO*L**2*YDR-P4*0.5*RHO*L**3*NDR)/D 
BB2=( P3*0 . 5*RHO*L**3*NDR-P6*0. 5*RHO*L**2*YDR)/D 

A1  -BB1*U*U 
Bl  -BB2*U*U 

Cl  — AD1-(AA11+AA22)*U 
A2  -(-AA12*BB2+AA22*BB1 ) *U**3 
B2  -(-AA21*BB1+AA11*BB2)*U**3 
Kl-AD3/( ( BB2*AA11-BB1*AA21 ) *U**3 ) 

C2  -AD2-(-AAl2*AA21+AAll*AA22 )*U**2+BB2*U*U*K1 
K2-( Cl*B2-C2*Bl )/( Al *B2-A2 *Bl ) 
K3-(C2*A1-C1*A2)/(A1*B2-A2*B1 ) 
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J  *0 
IJ=0 
JE-0 
OFF=0 
C 

DRHAT=0 . 0 
DRBAR-0.0 

SIMULATION  BEGINS 

DO  100  I-1,ISIM 

CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 

DO  600  K-1,9 
UCF-V+X{ K) *R 
SGN-1 .0 

IF  (UCF.LT.0.0)  SGN— 1.0 
VECl ( K)-HH(K) *UCF*UCF*SGN 
VEC2( K)-HH( K) *UCF*UCF*SGN*X( K) 

600  CONTINUE 

CALL  TRAP (9, VECl, X, SWAY) 

CALL  TRAP(9,VEC2,X,YAW) 

SWAY=-0 . 5*RH0*CDY*SWAY 
YAW  “-0 . 5*RH0*CDY*YAW 

FORCE  EQUATIONS 

FP2  -  -MASS*U*R+0.5*RHO*L**3*YR*U*R+0.5*RHO*L*L*( 

1  YV*U*V+YDR*U*U*DR)+SWAY 

FP3  -  -MASS*XG*U*R+0.5*RHO*L**4*NR*U*R+0.5*RHO*L**3* 

1  (NV*U*V+NDR*U*U*DR)+YAW 

VDOT  -( P5*FP2-P4*FP3)/( P5*P3-P4*P6 ) 

ROOT  -( P6*FP2-P3*FP3 )/( P4*P6-P3*P5) 

PSIDOT-R 

YDOT  -U*SIN( PSI )+V*COS( PSI )+VC 
XDOT  -U*COS( PSI )-V*SIN( PSI ) 

FIRST  ORDER  INTEGRATION 

PSI  -PSI  +DELTA*PSIDOT 
V  -V  +DELTA*VDOT 
R  -R  +DELTA*RDOT 
XPOS=XPOS+DELTA*XDOT 
YPOS=YPOS+ DELTA* YDOT 

YCTE»YPOS 
XAWAY-( XPOS-XD*L) 

C 

IF  ( (XAWAY) .GE.-(XYTURN*L) )  OFF-1 
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non  n  nan  n  non 


RUDDER  INPUT  CALCULATION 
YA=ABS( YPOS ) 

HDM=ATAN ( ( YPOS ) / ( -TARGET ) ) 

DR=K1*(PSI-HDM)+K2*V+K3*R 

IF  (DR.GT.  0.4)  DR-  0.4 
IF  (DR.LE.-0.4)  DR— 0.4 

PRINT  RESULTS 

JE-JE+1 

IF  ( JE.NE.IECHO)  GO  TO  99 
WRITE  (*,*)  '  XAWAY  XAWAY/L 

JE-0 

99  J-J+1 

IF  (J.NE.IPRNT)  GO  TO  100 
IJ-IJ+1 
TIME«I*DELTA 
XP-XPOS/L 
YP-YPOS/L 

XI-X2 ( M ) +XC+XP*COS ( -ALPHAl ) +YP*SIN( -ALPHAl ) 
YI=Y2(M)+YC+YP*COS(-ALPHAl ) -XP*SIN ( -ALPHAl ) 

WRITE  (11,*)  XI, YI 
0-0 

IF  (OFF.EQ.l)  GO  TO  500 
0  CONTINUE 

500  PSl-PSll 

XO-Xl 
YO-YI 
CONTINUE 
STOP 
END 

SUBROUTINE  TRAP ( N , A , B , OUT ) 

NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPE20IDAL  RULE 

DIMENSION  A( 1 ) ,B( 1 ) 

Nl-N-1 
OUT- 0.0 
DO  1  I-1,N1 

OUT1-0.5*(A(I)+A(I+1) )*(B(I+1)-B(I) ) 

OUT  -OUT+OUTl 
1  CONTINUE 
RETURN 
END 
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